#include <geometry_msgs/msg/pose_stamped.hpp>
#include <fishbot_behavior_tree/fishbot_behavior_tree.hpp>
#include "fishbot_navigation_behaviors/nav2pose_action_client.hpp"

namespace fishbot
{
    namespace behaviors
    {
        Nav2PoseActionClient::Nav2PoseActionClient(const std::string &name, const BT::NodeConfiguration &config,
                                                   const std::shared_ptr<fishbot::behavior_tree::BehaviorContext> &shared_resources)
            : fishbot::behavior_tree::ActionClientBehaviorBase<NavigationToPose>(name, config, shared_resources)
        {
        }

        BT::PortsList Nav2PoseActionClient::providedPorts()
        {
            return BT::PortsList({BT::InputPort<std::string>("action_name"),
                                  BT::InputPort<geometry_msgs::msg::PoseStamped>("pose"),
                                  BT::InputPort<std::string>("behavior_tree"),
                                  BT::InputPort<std::string>("goal_result_timeout"),
                                  BT::OutputPort<bool>("success")});
        }

        tl::expected<NavigationToPose::Goal, std::string> Nav2PoseActionClient::createGoal()

        {
            const auto maybe_pose = getInput<geometry_msgs::msg::PoseStamped>("pose");
            geometry_msgs::msg::PoseStamped pose;
            if (const auto error = fishbot::behavior_tree::maybe_error(maybe_pose))
            {
                return tl::make_unexpected("No input port [pose] provided");
            }
            else
            {
                pose = maybe_pose.value();
            }

            const auto maybe_behavior_tree = getInput<std::string>("behavior_tree");
            std::string behavior_tree;
            if (const auto error = fishbot::behavior_tree::maybe_error(maybe_behavior_tree))
            {
                behavior_tree = "";
            }
            else
            {
                behavior_tree = maybe_behavior_tree.value();
            }

            return nav2_msgs::build<NavigationToPose::Goal>().pose(pose).behavior_tree(behavior_tree);
        }

        tl::expected<bool, std::string> Nav2PoseActionClient::processResult(const std::shared_ptr<NavigationToPose::Result> result)
        {
            return true;
        }
    }
}